from ui_controller import RemoteUIController
import time
import os, cv2
import sys
import logging
from logger import logger
import time  # 确保 time 模块已导入

from go2_controller import Go2Controller

import os
os.environ["DISPLAY"] = ":0"


class RemoteController:
    def __init__(self, theme_name="gray_theme", debug_mode=False):
        self.ui = None
        self.go2 = None   # 🟢 用于存放Go2控制对象
        self.setup_ui(theme_name)
    
    def setup_ui(self, theme_name="gray_theme", language="zh"):
        """设置UI界面，支持主题和语言切换"""
        try:
            # 创建UI控制器 - 启用全屏模式
            self.ui = RemoteUIController(width=480, height=800, fullscreen=True, theme_name=theme_name, language=language)
            
            # 先创建窗口
            if not self.ui.create_window():
                self.ui.log_message(self.ui.texts["window_failed"], "error")
                return False
                
            # 添加控制按钮
            self.ui.add_button(self.ui.texts["button_stand"], 120, 150, 120, 70, "primary", self.stand_up_callback)
            self.ui.add_button(self.ui.texts["button_lie"], 360, 150, 120, 70, "primary", self.lie_down_callback)
            
            self.ui.add_button(self.ui.texts["button_scrape"], 120, 250, 120, 70, "primary", self.scrape_callback)
            self.ui.add_button(self.ui.texts["button_stretch"], 360, 250, 120, 70, "primary", self.stretch_callback)
            
            self.ui.add_button(self.ui.texts["button_happy"], 120, 350, 120, 70, "primary", self.happy_callback)
            self.ui.add_button(self.ui.texts["button_relax"], 360, 350, 120, 70, "primary", self.relax_callback)
            
            self.ui.add_button(self.ui.texts["button_demo1"], 120, 450, 120, 70, "primary", self.demo_pose1_callback)
            self.ui.add_button(self.ui.texts["button_demo2"], 360, 450, 120, 70, "primary", self.demo_pose2_callback)
            
            self.ui.add_button(self.ui.texts["button_exit"], 240, 550, 160, 70, "danger", self.exit_callback)
            
            # 强制刷新 UI
            self.ui.draw_interface()
            
            return True
        except Exception as e:
            logger.exception("设置UI失败")
            return False
    # 在回调函数上方）加入以下方法：
    def ensure_go2_connected(self):
        """确保Go2机器人控制连接已建立"""
        if self.go2 is None:
            try:
                self.ui.log_message(self.ui.texts["go2_connecting"], "info")
                from go2_controller import Go2Controller
                self.go2 = Go2Controller(interface="enx00e0986113a6")
                self.ui.log_message(self.ui.texts["go2_connected"], "success")
            except Exception as e:
                self.ui.log_message(f"{self.ui.texts['go2_failed']}: {e}", "error")
                return False
        return True
    
    
    # 按钮回调函数
    def scrape_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return

            self.go2.scrape()
            time.sleep(0.2)
            self.ui.log_message(self.ui.texts["command_completed"], "success")
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(self.ui.texts["command_failed"], "error")
    
    def stretch_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return

            self.go2.stretch()
            time.sleep(0.2)
            self.ui.log_message(self.ui.texts["command_completed"], "success")
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(f"执行拉伸命令失败: {e}", "error")
    
    def happy_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return

            self.go2.happy()
            time.sleep(0.2)
            self.ui.log_message(self.ui.texts["command_completed"], "success")
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(f"执行开心命令失败: {e}", "error")
    
    def relax_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return

            self.go2.relax()
            time.sleep(0.2)
            self.ui.log_message(self.ui.texts["command_completed"], "success")
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(f"执行放松模式命令失败: {e}", "error")
    
    def stand_up_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            # TODO: 添加起立控制代码
            # 确保已连接
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return

            self.go2.stand_up()
            
            time.sleep(0.2)
            self.ui.log_message(self.ui.texts["command_completed"], "success")
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(f"执行起立命令失败: {e}", "error")
    
    def lie_down_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            # TODO: 添加趴下控制代码
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return

            self.go2.stand_down()
            time.sleep(0.2)
            self.ui.log_message(self.ui.texts["command_completed"], "success")
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(f"执行趴下命令失败: {e}", "error")
    
    def demo_pose1_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return False

            if not self.go2.Dance1():
                self.ui.log_message(self.ui.texts["command_failed"], "error")
                return False

            self.ui.log_message(self.ui.texts["command_completed"], "success")
            return True
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(f"执行演示1姿态命令失败: {e}", "error")
            return False
    
    def demo_pose2_callback(self, button_name):
        try:
            self.ui.log_message(self.ui.texts["command_executed"], "info")
            if not self.ensure_go2_connected():
                self.ui.log_message(self.ui.texts["go2_not_connected"], "error")
                return

            self.go2.Dance2()
            time.sleep(0.2)
            self.ui.log_message(self.ui.texts["command_completed"], "success")
        except Exception as e:
            logger.exception(f"{self.ui.texts['command_failed']}: {e}")
            self.ui.log_message(f"执行演示2姿态命令失败: {e}", "error")
    
    def exit_callback(self, button_name):
        try:
            print(f"[DEBUG] 退出回调被触发，来源: {button_name}")
            self.ui.log_message(self.ui.texts["exit_message"], "info")
            self.ui.running = False
            
            # 强制关闭窗口
            if self.ui.window_created:
                print("[DEBUG] 正在关闭CV2窗口")
                cv2.destroyAllWindows()
                self.ui.window_created = False
            
            # 异步关闭Go2连接
            if self.go2:
                print("[DEBUG] 正在关闭Go2连接")
                import threading
                threading.Thread(target=self.go2.close).start()
            
            # 确保程序退出
            print("[DEBUG] 正在退出程序")
            sys.exit(0)
            t = threading.Thread(target=self._async_shutdown)
            t.daemon = True  # 设置为守护线程
            t.start()
            t.join(timeout=2.0)  # 等待线程完成，最多2秒
        except Exception as e:
            logger.exception(f"退出程序失败: {e}")
    
    def _async_shutdown(self):
        """异步关闭Go2连接"""
        try:
            if self.go2:
                self.go2.shutdown()
                self.go2 = None
        except Exception as e:
            logger.exception(f"关闭Go2连接失败: {e}")
    
    def run_with_cv_display(self):
        """尝试使用CV2显示"""
        try:
            # 绘制初始界面
            print("[DEBUG] 正在绘制初始界面...")
            self.ui.draw_interface()
            self.ui.log_message("System started", "success")
            self.ui.log_message("Waiting for command", "info")
            print("[DEBUG] 进入主循环 (cv2.imshow)")
            
            # 强制主循环运行（忽略初始 running 状态）
            self.ui.running = True
            loop_count = 0
            while True:
                loop_count += 1
                try:
                    # 检查窗口是否有效
                    if not self.ui.window_created:
                        if self.debug_mode:
                            print("[DEBUG] 窗口未创建，退出循环")
                        break
                        
                    cv2.imshow("Remote", self.ui.get_canvas())
                    key = self.ui.wait_key(30)
                    
                    # 键盘快捷键
                    if key == ord('q') or key == 27:
                        print("[DEBUG] 退出信号: 用户按下退出键 (q/ESC)")
                        break
                    elif key == ord('w'):
                        self.forward_callback("Key W")
                    elif key == ord('s'):
                        self.backward_callback("Key S")
                    elif key == ord('a'):
                        self.left_callback("Key A")
                    elif key == ord('d'):
                        self.right_callback("Key D")
                    elif key == ord('1'):
                        self.special_action1_callback("Key 1")
                    elif key == ord('2'):
                        self.special_action2_callback("Key 2")
                except Exception as e:
                    print(f"[DEBUG] 主循环异常: {str(e)}")
                    break
            
            print("[DEBUG] 退出主循环，关闭窗口")
            cv2.destroyAllWindows()
            
        except Exception as e:
            logger.exception(f"CV2显示模式失败: {e}")
            self.run_without_display()
    
    def run_without_display(self):
        """在没有显示的情况下运行（仅控制台）"""
        logger.info("进入控制台模式")
        
        try:
            import termios
            import tty
            import sys
            import select
            
            print("Running in console mode. Use keyboard commands:")
            print("W - Forward, S - Backward, A - Left, D - Right")
            print("1 - Action1, 2 - Action2, Q - Quit")
            
            self.ui.log_message("Console mode started", "info")
            
            # 设置终端为非阻塞模式
            fd = sys.stdin.fileno()
            old_settings = termios.tcgetattr(fd)
            
            try:
                tty.setraw(sys.stdin.fileno())
                
                while self.ui.is_running():
                    # 检查输入
                    if sys.stdin in select.select([sys.stdin], [], [], 0.1)[0]:
                        key = sys.stdin.read(1)
                        
                        if key == 'q' or key == '\x1b':  # ESC
                            break
                        elif key == 'w':
                            self.forward_callback("Key W")
                        elif key == 's':
                            self.backward_callback("Key S")
                        elif key == 'a':
                            self.left_callback("Key A")
                        elif key == 'd':
                            self.right_callback("Key D")
                        elif key == '1':
                            self.special_action1_callback("Key 1")
                        elif key == '2':
                            self.special_action2_callback("Key 2")
                        elif key == ' ':
                            self.ui.log_message("System running", "info")
                    
                    # 定期更新状态
                    time.sleep(0.1)
                        
            finally:
                termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
                
        except ImportError:
            # 如果termios不可用（如Windows），使用简单输入
            logger.warning("Termios不可用，使用简单输入模式")
            
            while self.ui.is_running():
                try:
                    # 非阻塞输入检查
                    import select
                    if select.select([sys.stdin], [], [], 0.1)[0]:
                        key = sys.stdin.read(1)
                        
                        if key == 'q':
                            break
                        elif key == 'w':
                            self.forward_callback("Key W")
                        elif key == 's':
                            self.backward_callback("Key S")
                        elif key == 'a':
                            self.left_callback("Key A")
                        elif key == 'd':
                            self.right_callback("Key D")
                        elif key == '1':
                            self.special_action1_callback("Key 1")
                        elif key == '2':
                            self.special_action2_callback("Key 2")
                except:
                    pass
                
                time.sleep(0.1)
    
    def run(self):
        """运行主程序"""
        try:
            # 首先尝试使用CV2显示
            self.run_with_cv_display()
        except Exception as e:
            logger.exception(f"显示模式失败: {e}")
            # 如果失败，回退到控制台模式
            self.run_without_display()
        
        # 清理资源
        try:
            self.ui.close()
            self.ui.log_message("Program safely exited", "success")
            logger.info("程序安全退出")
        except Exception as e:
            logger.exception(f"程序退出时发生错误: {e}")

# 调试模式开关（可通过环境变量或配置文件动态设置）
DEBUG_MODE = False  # 默认关闭

def debug_print(*args, **kwargs):
    """仅在调试模式下打印日志"""
    if DEBUG_MODE:
        print("[DEBUG]", *args, **kwargs)

def main():
    try:
        logger.info("程序启动")
        debug_print("正在初始化 RemoteController...")
        controller = RemoteController(debug_mode=DEBUG_MODE)
        controller.ui.debug_mode = DEBUG_MODE  # 确保传递到UI控制器
        print(f"[DEBUG] UI 初始化状态: {controller.ui is not None}")
        if controller.ui and controller.ui.window_created:
            print("[DEBUG] 窗口创建成功，准备进入主循环")
            controller.run()
        else:
            logger.error("无法创建窗口，请检查显示环境")
            print("[DEBUG] 窗口创建失败，可能原因: 显示环境不支持或 cv2.imshow 错误")
    except Exception as e:
        logger.exception(f"程序运行失败: {e}")
        print(f"[DEBUG] 主程序异常: {str(e)}")
    finally:
        logger.info("程序结束")
        print("[DEBUG] 程序终止")

if __name__ == "__main__":
    main()